/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <global_planner/grid_path.h>
#include <algorithm>
//搜索到路径后使用回溯grid_path（栅格路径）
#include <stdio.h>
namespace global_planner {
//sukai 从可行点中提取路径，从potential 代价数组中提取路径；//start_x, start_y, goal_x, goal_y 像素坐标；//path 路径点集合
bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
    std::pair<float, float> current;//定义一个pair类型的变量current，用于存储当前点的坐标。
    current.first = end_x;//将终点的x坐标赋值给current的first成员。
    current.second = end_y;//将终点的y坐标赋值给current的second成员。

    int start_index = getIndex(start_x, start_y);//将起点的坐标转化为数组下标，并赋值给start_index。  start_x, start_y, goal_x, goal_y 像素坐标

    path.push_back(current);//将终点加入路径中。
    int c = 0;//定义一个计数器c，用于记录路径长度。
    int ns = xs_ * ys_;//计算地图大小。
    
    while (getIndex(current.first, current.second) != start_index) {// 当当前点不是起点时，执行循环。
        float min_val = 1e10;//定义一个变量min_val，用于存储周围点的最小权值。
        int min_x = 0, min_y = 0;//定义两个变量min_x和min_y，用于存储周围点的坐标。
        for (int xd = -1; xd <= 1; xd++) {//循环遍历周围的点。
            for (int yd = -1; yd <= 1; yd++) {//循环遍历周围的点。
                if (xd == 0 && yd == 0)//排除当前点。
                    continue;
                int x = current.first + xd, y = current.second + yd;//计算周围点的坐标。
                int index = getIndex(x, y);//将周围点的坐标转化为数组下标。
                if (potential[index] < min_val) {//如果周围点的权值小于min_val。
                    min_val = potential[index];//将周围点的权值赋值给min_val。
                    min_x = x;//将周围点的x坐标赋值给min_x。
                    min_y = y;//将周围点的y坐标赋值给min_y。
                }//结束if语句。
            }//结束yd的循环。
        }//结束xd的循环
        if (min_x == 0 && min_y == 0)//如果没有找到周围点，返回false。
            return false;
        current.first = min_x;//将周围点的x坐标赋值给current的first成员。
        current.second = min_y;// 将周围点的y坐标赋值给current的second成员。
        path.push_back(current);//将周围点加入路径中。
        
        if(c++>ns*4){//如果路径长度超过了地图大小的4倍，返回false。
            return false;
        }//结束if语句。

    }// 结束while循环。
    return true;//如果找到了起点，返回true，否则返回false。
}

} //end namespace global_planner


/**
这段代码是一个A*算法的实现，用于寻找两点之间的最短路径。其中potential数组是每个点的权值，start_x, start_y, end_x, end_y是起点和终点的坐标，path是存储路径的vector。getIndex函数是将坐标转化为数组下标的函数。

算法的具体实现过程是：

将终点加入路径中。
从终点开始，每次找到周围权值最小的点，将其加入路径中，直到找到起点或者路径长度超过了地图大小的4倍。
如果找到了起点，返回true，否则返回false。
下面是代码的逐行解释：

std::pair<float, float> current; 定义一个pair类型的变量current，用于存储当前点的坐标。
current.first = end_x; 将终点的x坐标赋值给current的first成员。
current.second = end_y; 将终点的y坐标赋值给current的second成员。
int start_index = getIndex(start_x, start_y); 将起点的坐标转化为数组下标，并赋值给start_index。
path.push_back(current); 将终点加入路径中。
int c = 0; 定义一个计数器c，用于记录路径长度。
int ns = xs_ * ys_; 计算地图大小。
while (getIndex(current.first, current.second) != start_index) { 当当前点不是起点时，执行循环。
float min_val = 1e10; 定义一个变量min_val，用于存储周围点的最小权值。
int min_x = 0, min_y = 0; 定义两个变量min_x和min_y，用于存储周围点的坐标。
for (int xd = -1; xd <= 1; xd++) { 循环遍历周围的点。
for (int yd = -1; yd <= 1; yd++) { 循环遍历周围的点。
if (xd == 0 && yd == 0) 排除当前点。
int x = current.first + xd, y = current.second + yd; 计算周围点的坐标。
int index = getIndex(x, y); 将周围点的坐标转化为数组下标。
if (potential[index] < min_val) { 如果周围点的权值小于min_val。
min_val = potential[index]; 将周围点的权值赋值给min_val。
min_x = x; 将周围点的x坐标赋值给min_x。
min_y = y; 将周围点的y坐标赋值给min_y。
} 结束if语句。
} 结束yd的循环。
} 结束xd的循环。
if (min_x == 0 && min_y == 0) 如果没有找到周围点，返回false。
current.first = min_x; 将周围点的x坐标赋值给current的first成员。
current.second = min_y; 将周围点的y坐标赋值给current的second成员。
path.push_back(current); 将周围点加入路径中。
if(c++>ns*4){ 如果路径长度超过了地图大小的4倍，返回false。
return false; 结束函数并返回false。
} 结束if语句。
} 结束while循环。
return true; 如果找到了起点，返回true，否则返回false。

 */